#include "perspective_model.h"
#include "b_spline3.h"
#include "matrix4x4.h"
#include "surface_utils.h"
#include "top_model.h"
#include "height_model.h"
#include <iostream>

const unsigned int RollerCoasterModel::columnStep = 2;
const float RollerCoasterModel::columnWidth 	= 0.4f;
const float RollerCoasterModel::columnDepth 	= 0.4f;
const float RollerCoasterModel::railWidth 	= 0.08f;
const float RollerCoasterModel::railHeight 	= 0.08f;

RollerCoasterModel::RollerCoasterModel(Model& hmodel,Model& tmodel)
	:	hModel(hmodel),tModel(tmodel){

}

RollerCoasterModel::~RollerCoasterModel(){

}


void RollerCoasterModel::calculate(){
	railPath.clear();

	TopModel& tmodel = dynamic_cast<TopModel&>(tModel);
	HeightModel& hmodel = dynamic_cast<HeightModel&>(hModel);

	CurveModel::Points::const_iterator tpoints = tmodel.pointsBegin();

	unsigned int hControlSize = hmodel.controlSize();
	unsigned int tControlSize = tmodel.controlSize();
	float nu =  tControlSize+1;
	float nw =  hControlSize/4;

	for(unsigned int u=0;u<tmodel.bufferSize()-1;++tpoints,++u){
		float w = (nw/nu)*((float)u/BSpline3::step);
		Point ref(-tmodel.getXRef(),-tmodel.getYRef(),-hmodel.getGroundRef());
		Point p(tpoints->x1,tpoints->x2,hmodel.calculatePoint(w).x2);
		Point final = (p+ref)*tmodel.getScale();

		this->railPath.push_back(final);
	}
	this->railPath.push_back(this->railPath.front());

	calculateRailPoints();
	calculateColumns();
	calculateSurfacePoints();
	getIndices();
}

void RollerCoasterModel::calculateColumns(){
	unsigned int w=0;
	columns.clear();
	while(w<railPath.size()-1){
		Point pa=railPath[w];
		Point pb;

		if((w+1)<railPath.size()){
			pb = railPath[w+1];
		}else{
			pb = railPath[0];
		}

		Point curveDir = pb-pa;
		float xyang = atan2(curveDir.x2,curveDir.x1);

		Column col(columnWidth,(pa.x3),columnDepth,pa,xyang*180/PI);

		// Si el riel esta al raz del piso no tiene sentido una columna
		if(col.getHigh()>0){
			columns.push_back(col);
		}

		w+=(unsigned int)floor((float)(BSpline3::step/(float)columnStep)+0.5f);
	}
}

void RollerCoasterModel::getPointCountByPlane(){
	TopModel& top=dynamic_cast<TopModel&>(this->tModel);
	for(unsigned int i=0;i<top.segmentCount();i++){
		this->vecPointCountBySegment.push_back(SurfaceUtils::pointsCountByPlane(top.getSurfaceType(i))*(getPlaneCount()));
	}
}

void RollerCoasterModel::calculateSurfacePoints(){
	TopModel& top=dynamic_cast<TopModel&>(this->tModel);
	unsigned int segment=0;
	bool changeSegment=false;
	std::vector<Point>::const_iterator it = railPath.begin();
	for(unsigned int c=0;it!=railPath.end();c++){
		if (c==BSpline3::step){
			c=0;
			segment++;
			changeSegment=true;
		}
		Point pa=*it;
		Point pb;

		++it;
		if(it!=railPath.end()){
			pb = *it;
		}else{
			pb=*(++(railPath.begin()));
		}
		Point curveDir = pb-pa;
		if ((changeSegment)){
			SurfaceUtils::calculateSurfacePoints(curveDir,pa,vecSurfacePoints,vecNormalSurface,top.getSurfaceType(segment-1));
			//SurfaceUtils::calculateSurfacePoints(curveDir,pa,vecSurfacePoints,vecNormalSurface,top.getSurfaceType(segment));
			changeSegment=false;
		}
		SurfaceUtils::calculateSurfacePoints(curveDir,pa,vecSurfacePoints,vecNormalSurface,top.getSurfaceType(segment));
	}

	getPointCountByPlane();

	SurfaceUtils::sortVectors(top.getTypes(),getPlaneCount(),this->vecSurfacePoints,this->vecNormalSurface);
}

void RollerCoasterModel::getIndices(){
	TopModel& top=dynamic_cast<TopModel&>(this->tModel);
	unsigned int nextIndex=0;
	for (unsigned int i=0;i<top.segmentCount();i++){
		SurfaceUtils::calculateIndices(this->vecIndex,getPlaneCount(),nextIndex,top.getSurfaceType(i));
		nextIndex+=SurfaceUtils::pointsCountByPlane(top.getSurfaceType(i))*(getPlaneCount());
	}
}

unsigned int RollerCoasterModel::getPlaneCount(){
	return BSpline3::step+1;
}

void RollerCoasterModel::calculateRailPoints(){
	railPoints.clear();
	std::vector<Point>::const_iterator it = railPath.begin();

	// Transformo los puntos de la curva en segmentos de cubos
	for(;it!=railPath.end();){
		Point pa=*it;
		Point pb;

		++it;
		if(it!=railPath.end()){
			pb = *it;
		}else{
			// El primer punto se repite 2 veces no es necesario un caso particular.
			break;
		}

		Point curveDir = pb-pa;
		float xyang = atan2(curveDir.x2,curveDir.x1);

		float m = sqrt(pow(curveDir.x1,2)+pow(curveDir.x2,2));
		float xzang = atan2(curveDir.x3,m);

		Matrix4x4 r1 = Matrix4x4(xyang,Point(0.0f,0.0f,1.0f));
		Matrix4x4 r2 = Matrix4x4(xzang,Point(0.0f,1.0f,0.0f));
		Matrix4x4 t3 = Matrix4x4(pa.x1,pa.x2,pa.x3);

		Matrix4x4 t;
		t *= r2;
		t *= r1;
		t *= t3;

		Point p[4] = {	Point(0,-railWidth/2,railHeight/2),
						Point(0,railWidth/2,railHeight/2),
						Point(0,railWidth/2,-railHeight/2),
						Point(0,-railWidth/2,-railHeight/2) };


		for(unsigned int c=0;c<4;c++){
			p[c]=t*p[c];
			railPoints.push_back(p[c]);
		}
	}
}

unsigned int RollerCoasterModel::getCurvePointCountBySegment(){
	return BSpline3::step ;
}

unsigned int RollerCoasterModel::getIndexSize(unsigned int segment){
	TopModel& topModel=dynamic_cast<TopModel&>(tModel);
	if(topModel.getSurfaceType(segment)!=SurfaceUtils::NONE)
		return ((this->getPlaneCount())*SurfaceUtils::pointsCountByPlane(topModel.getSurfaceType(segment))-(this->getPlaneCount()))*2;
	else
		return 0;
}

TopModel& RollerCoasterModel::getTopModel(){
	return dynamic_cast<TopModel&>(this->tModel);
}
